use strict;
use warnings;
use XML::Bare;

main();

sub help {
    print "Usage:\n";
    print "perl -w MakeWorld.pl WORK_FOLDER INCLUDE_FOLDER\n";
    print "WORK_FOLDER: contain robot.config.xml file\n";
    print "INCLUDE_FOLDER: contain include files\n";

    exit(1);
}

sub InvalidFile {
    my $fileName = shift;
    die "Invalid robot config file $fileName\n";
}

sub main {
    help() if not defined $ARGV[0];
    help() if not defined $ARGV[1];

    my ( $workFolder, $includeFolder ) = ( $ARGV[0], $ARGV[1]);

    # Parse robot.config.xml file
    my $xmlFile = $workFolder . "/robots.config.xml";
    print "Parsing $xmlFile file...\n";
    my $xml = new XML::Bare(file => $xmlFile)
        or die "Can't open $xmlFile file";
    my $config = $xml->parse();
    my $robotNumber = 0;
    my ( @robotPositions, @robotTypeList, @robotFileList );
    my %robotFileTag;

    # Generate robot position list
    if( $config->{xml}->{PlaceType}->{value} eq "Random" ) {
        my $robotGroupsRef;
        if ( ref $config->{xml}->{Random}->{number} eq 'ARRAY' ) { 
            $robotGroupsRef = $config->{xml}->{Random}->{number};
        }
        else {
            $robotGroupsRef = [ $config->{xml}->{Random}->{number} ];
        }

        print "Random place robots in environment\n";

        # Read map information
        my $map = {
            sizex=>$config->{xml}->{MapSize}->{x}->{value},
            sizey=>$config->{xml}->{MapSize}->{y}->{value},
            blocks=>$config->{xml}->{Random}->{obstacles}->{block}
        };
        print "Read map information: size $map->{sizex} x $map->{sizey}  
            " . scalar(@{$map->{blocks}}) . " blocks\n";

        # Generate available cell list
        print "Genrate available cells list\n";
        my @cells;
        foreach my $cellY(0 .. $map->{sizey} - 1) {
            foreach my $cellX(0 .. $map->{sizex} - 1) {
                push @cells, {x=>$cellX, y=>$cellY};
            }
        }
        
        # Remove obstacle cells
        print "Remove obstacle cells\n";
        foreach my $obstacle(@{$map->{blocks}}) {
            $cells[$obstacle->{x}->{value} + $obstacle->{y}->{value} * $map->{sizex}] = undef;
        }

        for(my $i = 0; $i < scalar(@cells); $i++) {
            if(not defined $cells[$i]) {
                splice(@cells, $i, 1);
                $i--;
            }
        }

        # Assign cell to robots
        print "Assign cells to robots\n";
        foreach ( @{$robotGroupsRef} ) {
            # Record robot file name
            my $robotFile = $_->{srcfile}->{value};
            push @robotFileList, $robotFile 
                if not defined $robotFileTag{$robotFile};
            $robotFileTag{$robotFile} = 1;
            
            foreach (0 .. $_->{value} - 1) {
                my $pos = int(rand(scalar(@cells)));
                $cells[$pos]->{x} += 0.5;
                $cells[$pos]->{y} += 0.5;
                print "robot $robotNumber get cell($cells[$pos]->{x} $cells[$pos]->{y})\n";
                push @robotPositions, $cells[$pos];
                splice(@cells, $pos, 1);
                push @robotTypeList, $robotFile;
                $robotNumber++;
            }
        }
    }
    elsif( $config->{xml}->{PlaceType}->{value} eq "Static" ) {
        print "Static place robot in environment\n";
        foreach(@{$config->{xml}->{Static}->{Robots}->{robot}}) {
            print "robot $robotNumber get cell($_->{x}->{value} $_->{y}->{value})\n";
            push @robotPositions, {x=>$_->{x}->{value}, y=>$_->{y}->{value}};
            push @robotTypeList, $_->{srcfile}->{value};

            # Record robot file name
            push @robotFileList, $_->{srcfile}->{value} 
                if not defined $robotFileTag{$_->{srcfile}->{value}};
            $robotFileTag{$_->{srcfile}->{value}} = 1;
            $robotNumber++;
        }
    }
    else {
        InvalidFile($xmlFile);
    }

    # Load world file
    my $worldFile = $workFolder . "/world";

    print "Load world file ($worldFile)\n";
    open WORLD_FILE, $worldFile 
        or die "Can't open $worldFile\n";
    my @worldLines = <WORLD_FILE>;
    close WORLD_FILE;
    
    # Load robot file
    my %robotLines;
    foreach (@robotFileList) {
        my $robotFile = $workFolder . "/" . $_;
     
        print "Load robot file ($robotFile)\n";
        open ROBOT_FILE, $robotFile
            or die "Can't open $robotFile";
    
        my @robotFile = <ROBOT_FILE>;
        close ROBOT_FILE;

        $robotLines{$_} = \@robotFile;
    }

    # Generate .world file
    my $_world = $workFolder . "/simulation.world";
    print "Generating $_world...\n";
    
    my $worldContent = join('', @worldLines);
    foreach my $robotId(0 .. $robotNumber - 1) {
        print "Generating robot$robotId...\n";
        my $robot = join('', @{$robotLines{$robotTypeList[$robotId]}});
        $robot =~ s/#NAME#/r$robotId/;
        $robot =~ s/#X#/$robotPositions[$robotId]->{x}/;
        $robot =~ s/#Y#/$robotPositions[$robotId]->{y}/;
        $robot =~ s/#A#/0/;
        $robot =~ s/#IP#/192.168.1.$robotId/;
        $robot =~ s/#MAC#/66:66:66:66:66:$robotId/;
        $worldContent .= $robot;
    }

    # Write .world
    open WORLD, ">".$_world
        or die "Can't open file $_world";
    print WORLD $worldContent;
    close WORLD;
}
